﻿//MIT License
//Copyright (c) 2024 nuaazgg, Nanjing University of Aeronautics and Astronautics(NUAA)

using System.Collections;
using System.Collections.Concurrent;
using System.Collections.Generic;
using UnityEngine;

public class VirtualArm : MonoBehaviour
{

    public GameObject[] joints;//从小拇指外端到内端，从小拇指到大拇指
    public ConcurrentQueue<double[]> jointAngles = new ConcurrentQueue<double[]>();
    private Vector3[] initAngle;
    public enum Axis
    {
        X,
        Y,
        Z,
    }
    public Axis[] axis;
    float temp=0f;
    // Start is called before the first frame update
    void Start()
    {
        initAngle = new Vector3[6];
        for (int i = 0; i < 6; ++i)
        {
            initAngle[i] = joints[i].transform.localEulerAngles;
            //Debug.Log(initAngle[i]);
        }
        //fingers[0].transform.position
    }

    // Update is called once per frame
    void Update()
    {
        if (jointAngles.TryDequeue(out var angles))
        {
            /*            joints[0].transform.localRotation = Quaternion.Lerp(joints[0].transform.localRotation, Quaternion.Euler(initAngle[0] + Vector3.down * (float)angles[0] / Mathf.PI * 180f), Time.deltaTime);
                        joints[1].transform.localRotation = Quaternion.Lerp(joints[1].transform.localRotation, Quaternion.Euler(initAngle[1] + Vector3.left * (float)angles[1] / Mathf.PI * 180f), Time.deltaTime);
                        joints[2].transform.localRotation = Quaternion.Lerp(joints[2].transform.localRotation, Quaternion.Euler(initAngle[2] + Vector3.left * (float)angles[2] / Mathf.PI * 180f), Time.deltaTime);
                        joints[3].transform.localRotation = Quaternion.Lerp(joints[3].transform.localRotation, Quaternion.Euler(initAngle[3] + Vector3.up * (float)angles[3] / Mathf.PI * 180f), Time.deltaTime);
                        joints[4].transform.localRotation = Quaternion.Lerp(joints[4].transform.localRotation, Quaternion.Euler(initAngle[4] + Vector3.right * (float)angles[4] / Mathf.PI * 180f), Time.deltaTime);
                        joints[5].transform.localRotation = Quaternion.Lerp(joints[5].transform.localRotation, Quaternion.Euler(initAngle[5] + Vector3.left * (float)angles[5] / Mathf.PI * 180f), Time.deltaTime);*/
            joints[0].transform.localRotation = Quaternion.Euler(initAngle[0] + Vector3.down * (float)angles[0] / Mathf.PI * 180f);
            joints[1].transform.localRotation = Quaternion.Euler(initAngle[1] + Vector3.left * (float)angles[1] / Mathf.PI * 180f);
            joints[2].transform.localRotation = Quaternion.Euler(initAngle[2] + Vector3.left * (float)angles[2] / Mathf.PI * 180f);
            joints[3].transform.localRotation = Quaternion.Euler(initAngle[3] + Vector3.up * (float)angles[3] / Mathf.PI * 180f);
            joints[4].transform.localRotation = Quaternion.Euler(initAngle[4] + Vector3.right * (float)angles[4] / Mathf.PI * 180f);
            joints[5].transform.localRotation = Quaternion.Euler(initAngle[5] + Vector3.left * (float)angles[5] / Mathf.PI * 180f);

        }
    }
}
